Source of Acquisition 
NASA Ames Research Center 


The item to be cleared is a low-fidelity software simulation model of a hypothetical free- 
fiying robot designed for use in zero gravity environments. 

This simulation model works with the HCC simulation system that was developed by 
Xerox PARC and NASA Ames Research Center. HCC has been previously cleared for 
distribution. 

When used with the HCC software, the model computes the location and orientation of 
the simulated robot over time. Failures (such as a broken motor) can be injected into the 
simulation to produce simulated behavior corresponding to the failure. 

Release of this simulation will allow researchers to test their software diagnosis systems 
by attempting to diagnose the simulated failure from the simulated behavior. 

This model does not contain any encryption software nor can it perform any control tasks 
that might be export controlled. 
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* This tiLe is the HOC code tor the PSA simulation. PSA was officially 
renamed to SMR, but the name PSA has stuck for the project. Both names 
are used for the project, and both might be around in code below. */ 

Vmodule "mathlib" 

# i f de f EXTERNAL JTONTROLLER 
imodule l, psa_control ler_Sdof '* 

%max_step 0.01 
tfendif 

/* define constants used in code below */ 

^define m 5 // mass of the PSA kg 

^define rad 0.1 // radius of the PSA m 

/* define constants used to control behavior of HCC */ 

/* see HCC programmer's manual for explaination of these */ 

//%MEMORY_MAX 8000000 
/ / %EPSILON l.Qe-11 
//%INTEGRATION_INIT 0.0001 
//%SAMPLE_INTERVAL_MIN 0.01 

/ / %SAMPLE__INTERVAL_MAX 0.01 // Good number = 0.01 

/******************************************************* 

CFan class definition 

•A-*****************************************************/ 

class CFan { 

/* Description of variables: 

kl Curvefit constant for thrust of fan as 

function of fan speed 

k2 Curvefit for modeling aerodynamic torque 

on motor as function of fan speed 
inertia Rotational inertia of fan along spin axis 

tm Torque constant of motor 

{torque = tm * current) 

vel Angular velocity of fan (rad/s) 

current Current sent to motor by controller 

force Thrust on PSA due to fan 

moment Total torque provided by motor 

aero_torque Torque of fan used to provide thrust, 

overcome drag, overcome friction, etc. 
h Angular momentum of fan */ 

public interval vel, force, moment , aero_torque, h; 

public CFan ( interval i, interval kl, interval k2, interval inertia, 
interval tm, interval current) { 

vel = 0 . 0 ; 
always { 

/* Fan model with fan dynamics, kl and k2 are two constants 
determined empirically and passed in to the object. 

This model has the torque created by the fan motor being 
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" " pub L l * X'yrMnu ! mr. er/ u i, interval. maJ.J, inter'/ il radius, 

* iiif.'-fM L mom_of_La, interval ;<_LnLt, mterviL y _ i. n i. t , interval, z^iain, 
mter/ii ql^Lmt, iai-^rv.il q2_mit, interval q3_Lnit, interval q4_init, 

CFan FO, CFan F l , IF in F2 # CFan F3, CFan F4 , CFan FS) f 

:< = :<_init; y = y_i.nl t; z = z_init; 

ql = q L _ 1 n i t ; q2 = q2_init; q3 = q3_init; q4 = q4_init; 

U = 0 ; V - 0 ; w = 0 ; p = 0 ; q = 0 ; r = 0 ; 

always { 

/* Calculate total moment in each component direction */ 
l_moment : = ( F5 . force-F4 . force) *radius - F2 . aero_torque - F3 . aero__torque ; 
m_moment :» (FI . force- FO . force) *radius - F4 . aero_torque - F5 . aero_torque ; 
n__moment : = (F3 . £orce-F2 . force) *radius - FO .aero_torque - FI .aero^torque; 

/* Balance of moments in each component direction, used 
to find angular velocities (p, q, r) . */ 
p ' : = { l_moment - F2.h' - F3.h'+ F4.h*r + F5.h*r 

- F0.h*q - Fl.h*q) / mom_of_in; 

q' : = (m_moment - F4.h' - F5.h' - F2.h*r - F3.h*r 

+ F0.h*p + Fl.h*p) / mom_of_in; 
r ' : = (n^moment - FO.h' - Fl.h' + F2.h*q + F3.h*q 

- F4.h*p - F5.h*p) / mom_of_in; 

/* Calculate orientation based on (p, q, r) . Orientation is 
in quaternion form. */ 
ql' : = (q4*p-q3 *q+q2*r) / 2 ; 
q2 # : = (q3*p+q4*q-ql*r) /2; 
q3 ' : = (-q2*p+ql*q+q4*r) /2 ; 
q4 ' : = ( -ql*p-q2 *q-q3 *r ) /2 ; 

/* Calculate total force in each component direction */ 
x_f orce : =F2 . force + F3 . force ; 
y__force : =F4 . force + F5. force; 
z__f orce : =F0 . force + FI . force ; 

/* Balance of forces in each component direction, used 
to find (u, v, w) . Note the coriolis forces added as a 
result of the body- fixed coordinate system used in the 
force balance. */ 
u' ; = (x_force) /mass-2* (q*w-r*v) ; 
v' : = (y_force) /mass-2* (r*u-p*w) ; 
w' : = (z__force) /mass-2* (p*v-q*u) ; 

/* Convert the body- fixed velocities to global velocities 
and integrate to find global position (x, y, z) */ 
x' : =u* (1-2 *q2^2 - 2 *q3 ^2 ) +v*2* (ql*q2 -q3 *q4 ) +w*2* (q3 *ql+q2 *q4 ) ; 
y' : =u*2 * (ql*q2+q3*q4) +v* (l-2*q3^2-2*ql^2) +w*2* (q2 *q3 -ql*q4 ) ; 
z' : =u*2* (q3*ql-q2*q4) +v*2* (q2*q3+ql*q4) +w* (l-2*ql^2-2*q2^2 ) ; 

#ifdef EXTERNAL_CONTROLLER 

junk as update_state ( time, u, v, w, p, q, r/*, u' , v', w' , p' , q' , r'*/); 

ftendif 

} // end always 

sample (x, y, z,u, v, w, x',y',z',u',v',w', 
ql, q2 , q3 , q4 , p, q, r) ; 

} // end CDynamics constructor 
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HILL ’ L. 


i ■ J 


:d orue - 
xfotrce f* 




C uiJtoC'.y - 0 , ‘5 * ■' :d orue - (z torque / radLUS) 

'E mltora - 0.5 * ‘xforce f* (z torque / radius) ) 

fan4 force = 0.5 * (/noroe - ( ;< torque / radius) ) 

tuns force = 0.5 * (/force +• (:< torque / radius) ) 


r Apply a conversion factor to calculate the motor current for each 
fan, based on the desired thrust of the fan. This current is 
then sent to the fan of the PSA. */ 

// 3 .631 


fanOcurrent 

= 

3 . 19 

* 

fanOf orce 

fanlcurrent 

= 

3 . 19 

* 

fanlforce 

fan2current 

= 

3 . 19 

* 

fan2 force 

f an3current 

= 

3 . 19 

* 

fan3 force 

f an4current 

= 

8 . 19 

* 

fan4force 

f an5current 
// end always 

= 

3 . 19 

* 

fanSforce 


#endif 


} //end CController constructor 


} // end CController class def . 


^•kie-k-kie-kieitieieieie-itie-ir-kitic-kieieitie-k-kieicieie-k-k-kie'kicit-k-k'kic-k-kieie-kit-k-kieicif-k-k-kitrkic'k 

CPlanner class definition 

*****★*★**★*★★***★**★*'*★*★*★*★★*★*★****************-*■**★•*''*'*★ j 

/* The PlannerClass is used to send the desired velocities 

to the ControllerClass . Right now, this is a lookup table 
based on the simulation time. It could come from 
a more advanced path planner as well. 

Note that these are desired velocities, even though the 
variable names are just x, y, etc. */ 

class CPlanner { 

public interval xdes, ydes, zdes; 

public interval roll^des, pitch_des, yaw_d.es; 

public CPlanner(){ 
always { 

/* Set all values to 0 as the default, for all times where they 
are not defined in the list below. */ 

/* I had a problem with this working - after 43 seconds, these 
values end up being undefined. HCC won't crash, but the 
fan's force values become undefined, and the PSA will coast 
in the last direction it is facing. */ 

unless ( (time >= 0) | | (time <= 43) ) { 

xdes = 0.0; ydes = 0.0; zdes = 0.0; 
roll_des = 0.0; pitch_des = 0.0; yaw_des = 0.0; 

} 

if ( (time >= 0.0) && (time < 1.0) ) { 

xdes = 0.0; ydes = 0.0; zdes = 0.0; 
roll_des = 0.0; pitch_des = 0.0; yaw_des = 0.1; 

} 

/ * Send PSA forward */ 
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jji. .tun 

►s l . y tw t ; 

C UlO 

= 

new 

CF m f L . 0 ( 

3 . 0e - 3 , 
0 . 0004, 

2 . 0e - L 0 , L . Oe - 7 , 

cont ro L Ler . f anOcurrent > ; 

fan L 

- 

new 

CFan (2.0, 

3 . 0e-8, 
0 . 0004 , 

2 . 0e - L0 , i . 0e-7 , 

cont rol ler . fanlcurrent) ; 

fan2 

= 

new 

CFan (3.0, 

8 . 0e-8, 
0 . 0004, 

2.0e-10, 1.0e-7, 

controller . fan2current ' ; 

fan3 

- 

new 

CFan (4.0, 

3 . Oe-8, 
0 . 0004 , 

2.0e-10, 1.0e-7, 

controller . £an3current) ; 

f an4 

= 

new 

CFan (5.0, 

3 . 0e - 8 , 
0 . 0004, 

2.0e-10, 1.0e-7, 

controller . f an4current ) ; 

fans 

— 

new 

CFan (6.0, 

3 . Oe-8 , 
0 . 0004, 

2.0e-10, l.Oe-7, 

controller . fanScurrent) ; 


} 

X 

) 

#if def EXTERNAL_CONTROLLER 

interval junk = init_external_controller ( ) ; 

#endif 

/********************************************** 
Section to change depending on if using visualization 
*****★★**★★****★★***★**★★■***★★***★**■★*★***★*★★★/ 

#ifndef VISUALIZATION 
/^Global variable */ 

CPSA psaO; 

psaO - new CPSA(0 / 0, 0, 0, 0, 0, 1) ? 

#endif 
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